/**
 * 
 */
package cyclopean.world.zone;

import javax.media.opengl.GL;


public class Position
{
	// Rotation:
	static final int RX = 0;
	static final int UX = 1;
	static final int BX = 2;
	static final int RY = 4;
	static final int UY = 5;
	static final int BY = 6;
	static final int RZ = 8;
	static final int UZ = 9;
	static final int BZ = 10;
	
	// Location (in camera space):
	static final int TR = 12;
	static final int TU = 13;
	static final int TB = 14;

	Zone zone;
	float locX = 0;
	float locY = 0;
	float locZ = 0;
	final float[] matrix = new float[] {1, 0, 0, 0,
										0, 1, 0, 0,
										0, 0, 1, 0,
										0, 0, 0, 1};

	public Position(Zone zone)
	{
		this.zone = zone;
	}
	
	/**
     * @param other
     */
    public Position(Position other)
    {
	    this.zone = other.zone;
	    this.locX = other.locX;
	    this.locY = other.locY;
	    this.locZ = other.locZ;
	    this.matrix[ 0] = other.matrix[ 0];
	    this.matrix[ 1] = other.matrix[ 1];
	    this.matrix[ 2] = other.matrix[ 2];
	    this.matrix[ 3] = other.matrix[ 3];
	    this.matrix[ 4] = other.matrix[ 4];
	    this.matrix[ 5] = other.matrix[ 5];
	    this.matrix[ 6] = other.matrix[ 6];
	    this.matrix[ 7] = other.matrix[ 7];
	    this.matrix[ 8] = other.matrix[ 8];
	    this.matrix[ 9] = other.matrix[ 9];
	    this.matrix[10] = other.matrix[10];
	    this.matrix[11] = other.matrix[11];
	    this.matrix[12] = other.matrix[12];
	    this.matrix[13] = other.matrix[13];
	    this.matrix[14] = other.matrix[14];
	    this.matrix[15] = other.matrix[15];
    }

    public Position(Zone zone, float x, float y, float z)
    {
	    this.zone = zone;
	    this.locX = x;
	    this.locY = y;
	    this.locZ = z;
	    
	    calcTranslation();
    }

	/* (non-Javadoc)
     * @see java.lang.Object#clone()
     */
    @Override
    protected Position clone()
    {
	    return new Position(this);
    }

	/**
	 * Moves coordinate system relative to the absolute system.
	 * 
	 * @param dx Movement along the x-axis.
	 * @param dy Movement along the y-axis.
	 * @param dz Movement along the z-axis.
	 */
	public void move(float dx, float dy, float dz)
	{
		// TODO Apply portals
		locX += dx;
		locY += dy;
		locZ += dz;

		calcTranslation();
	}

	/**
     * 
     */
    private void calcTranslation()
    {
		matrix[TR] = -(matrix[RX] * locX + matrix[RY] * locY + matrix[RZ] * locZ);
		matrix[TU] = -(matrix[UX] * locX + matrix[UY] * locY + matrix[UZ] * locZ);
		matrix[TB] = -(matrix[BX] * locX + matrix[BY] * locY + matrix[BZ] * locZ);
    }

	/**
	 * Moves coordinate system relative to some coordinate system.
	 * 
	 * @param df Forward movement.
	 * @param dr Rightward movement.
	 * @param du Upward movement.
	 * @param sys Coordinate system in which to move. <code>null</code> defaults to <code>this</code>.
	 */
	public void move(float df, float dr, float du, Position sys)
	{
		final Position other = (sys == null) ? this : sys;

		final float dx = du * other.matrix[UX] + dr * other.matrix[RX] - df * other.matrix[BX];
		final float dy = du * other.matrix[UY] + dr * other.matrix[RY] - df * other.matrix[BY];
		final float dz = du * other.matrix[UZ] + dr * other.matrix[RZ] - df * other.matrix[BZ];
		
		move(dx, dy, dz);
	}
	
	public void getRelativePos(float df, float dr, float du, final Position system, final float[] output, final int offset)
    {
	    assert (output.length >= 3 + offset) : "There must be room for at least three elements in the output.";

        final Position other = (system == null) ? this : system;

        output[offset    ] = du * other.matrix[UX] + dr * other.matrix[RX] - df * other.matrix[BX];
        output[offset + 1] = du * other.matrix[UY] + dr * other.matrix[RY] - df * other.matrix[BY];
        output[offset + 2] = du * other.matrix[UZ] + dr * other.matrix[RZ] - df * other.matrix[BZ];
    }
	
	/**
	 * Rotates the coordinate system around its axes in a mouse-friendly way.
	 * 
	 * @param dr Amount to turn rightward.
	 * @param du Amount to turn upward.
	 */
	public void turn(float dr, float du)
	{
		float x = matrix[BX] - dr * matrix[RX] - du * matrix[UX];
		float y = matrix[BY] - dr * matrix[RY] - du * matrix[UY];
		float z = matrix[BZ] - dr * matrix[RZ] - du * matrix[UZ];
		float l = (float)(1.0 / Math.sqrt(x * x + y * y + z * z));

		// Backward
		matrix[BX] = x * l;
		matrix[BY] = y * l;
		matrix[BZ] = z * l;

		// Right = U x B
		matrix[RX] = matrix[UY] * matrix[BZ] - matrix[UZ] * matrix[BY];
		matrix[RY] = matrix[UZ] * matrix[BX] - matrix[UX] * matrix[BZ];
		matrix[RZ] = matrix[UX] * matrix[BY] - matrix[UY] * matrix[BX];

		// Up = B x R
		matrix[UX] = matrix[BY] * matrix[RZ] - matrix[BZ] * matrix[RY];
		matrix[UY] = matrix[BZ] * matrix[RX] - matrix[BX] * matrix[RZ];
		matrix[UZ] = matrix[BX] * matrix[RY] - matrix[BY] * matrix[RX];
		
		// Re-normalise
		x = matrix[RX];
		y = matrix[RY];
		z = matrix[RZ];
		l = (float)(1.0 / Math.sqrt(x * x + y * y + z * z));
		matrix[RX] *= l;
		matrix[RY] *= l;
		matrix[RZ] *= l;

		x = matrix[UX];
		y = matrix[UY];
		z = matrix[UZ];
		l = (float)(1.0 / Math.sqrt(x * x + y * y + z * z));
		matrix[UX] *= l;
		matrix[UY] *= l;
		matrix[UZ] *= l;

		calcTranslation();
	}
	
	public void setAngles(float heading, float pitch)
	{
		final double sinH = Math.sin(heading);
		final double cosH = Math.cos(heading);
		final double sinP = Math.sin(pitch);
		final double cosP = Math.cos(pitch);
		
		matrix[BX] = -(float)(cosH * cosP);
		matrix[BY] = -(float)(sinH * cosP);
		matrix[BZ] = -(float)(sinP);
		matrix[UX] = -(float)(cosH * sinP);
		matrix[UY] = -(float)(sinH * sinP);
		matrix[UZ] = (float)(cosP);
		matrix[RX] = (float)sinH;
		matrix[RY] = -(float)cosH;
		matrix[RZ] = 0;
		
		calcTranslation();
	}

    public void apply(GL gl)
    {
	    gl.glMultMatrixf(matrix, 0);
    }

    public Zone getZone()
    {
	    return zone;
    }
}